#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <vector>
#include <string>
 
#include  "api_share_ros.cpp"
using namespace std;
 
#include<iostream>
#include<pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include<pcl/point_types.h>
#include <chrono>
#include <thread>
using PointT = pcl::PointXYZI;
cv::Mat Img;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;

void send_pc(){
    share_men_instance.send_pc_array(cloud, 3);
}

void send_img(){
    share_men_instance.send_img(Img);
}

int main(int argc, char** argv) {
    
    //创建了一个名为cloud的指针，储存XYZ类型的点云数据
    cloud.reset(new pcl::PointCloud<pcl::PointXYZI>);

    //
    //*打开点云文件
    if (pcl::io::loadPCDFile<pcl::PointXYZI>("/home/st/ubuntu_data/NRSL/little_project/cpp_learning/linux_cpp_py_share_memory/test.pcd", *cloud) == -1) {
        PCL_ERROR("Couldn't read file rabbit.pcd\n");
        return(-1);
    }
    // std::cout << "Loaded:" << cloud->width*cloud->height<<"data points from test_pcd.pcd with the following fields:"<< std::endl;
    // for (size_t i = 0; i < cloud->points.size(); ++i) {
    //     std::cout << "      " << cloud->points[i].x << "   " << cloud->points[i].y << "   " << cloud->points[i].z << "   " << cloud->points[i].intensity<< "   " << std::endl;
    // }
    // system("pause");
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
    pcl::PassThrough<PointT> pass_filter;
    pass_filter.setInputCloud (cloud);//这个参数得是指针，类对象不行
    pass_filter.setFilterFieldName ("z");//设置想在哪个坐标轴上操作
    pass_filter.setFilterLimits (-10, 6);//将x轴的0到1范围内
    pass_filter.setFilterLimitsNegative (false);//保留（true就是删除，false就是保留而删除此区间外的）
    pass_filter.filter (*cloud);//输出到结果指针

    int points_size_max = 3;
    // if (cloud->points.size() < points_size_max){
    //     points_size_max = cloud->points.size();pc_array
    // }
    // int pc_mat_w = 200;
    // int pc_mat_h = ceil(points_size_max / pc_mat_w); 
    // Mat pc_mat= Mat::zeros(Size(pc_mat_h, pc_mat_w), CV_32FC4);
    // float pc_array[points_size_max * 4];
    for (int i = 0; i < points_size_max; ++i) {
        // pc_array[i *4 ] = cloud->points[i].x;
        // pc_array[i *4 + 1] = cloud->points[i].y;
        // pc_array[i *4 + 2] = cloud->points[i].z;
        // pc_array[i *4 + 3] = cloud->points[i].intensity;
        // pc_mat.at<Vec4f>(i / pc_mat_w,i % pc_mat_w)[0] = cloud->points[i].x;
        // pc_mat.at<Vec4f>(i / pc_mat_w,i % pc_mat_w)[1] = cloud->points[i].y;
        // pc_mat.at<Vec4f>(i / pc_mat_w,i % pc_mat_w)[2] = cloud->points[i].z;
        // pc_mat.at<Vec4f>(i / pc_mat_w,i % pc_mat_w)[3] = cloud->points[i].intensity;
        std::cout << "raw point      " << cloud->points[i].x << "   " << cloud->points[i].y << "   " << cloud->points[i].z << "   " << cloud->points[i].intensity<< "   " << std::endl;
    }
    // for(int i = 0;i < points_size_max * 4; i = i+4){
    //     std::cout << "raw array      " << pc_array[i] << "   "  << 
    //     pc_array[i +1] << "   " << 
    //     pc_array[i+2] << "   " << 
    //     pc_array[i+3]<< "   " << std::endl;
    // }
        // cout<< pc_mat <<endl;

    Img=cv::imread("/home/st/ubuntu_data/dataset/gazebo/city/0711/testing/image_2/000000.png",cv::IMREAD_COLOR);
    // imshow("test", Img);
    // waitKey(500);
    // cout<<Img<<endl;

    std::chrono::system_clock::time_point t1, t2;
    while(1){
        sleep(0.001);

        if(share_men_instance.p_share_data->pc_flag == 0){
            share_men_instance.send_pc_array(cloud, points_size_max);
            cout<<"point share size "<< share_men_instance.p_share_data->pc_size <<endl;
            // for(int i = 0;i < share_men_instance.p_share_data->pc_array_length; i = i+4){
            //     std::cout << "      " << share_men_instance.p_share_data->pc_array_data[i] << "   "  << 
            //     share_men_instance.p_share_data->pc_array_data[i +1] << "   " << 
            //     share_men_instance.p_share_data->pc_array_data[i+2] << "   " << 
            //     share_men_instance.p_share_data->pc_array_data[i+3]<< "   " << std::endl;
            // }
            for (int i = 0; i< share_men_instance.p_share_data->pc_size*4; i++){    
                cout<<"sahre "<<share_men_instance.p_share_data->pc_array_data[i]<<endl;
            }
        }

        if(share_men_instance.p_share_data->pc_flag == 3){
            cout<<"pc_pre_size "<<share_men_instance.p_share_data->pc_pre_size<<endl;
            for(int i=0; i< share_men_instance.p_share_data->pc_pre_size;i++){
                cout<<"pre data "<< share_men_instance.p_share_data->pc_pre_data[i]<<endl;
            }
            share_men_instance.p_share_data->pc_flag = 4;
        }

        // if(share_men_instance.p_share_data->img_flag == 0){
        //     share_men_instance.send_img(Img);
        // }

        // if(share_men_instance.p_share_data->img_flag == 3){
        //     cout<<"img_pre_size "<<share_men_instance.p_share_data->img_pre_object_size<<endl;
        //     for(int i=0; i<share_men_instance.p_share_data->img_pre_object_size; i++){
        //         cout<<"pre class  "<< share_men_instance.p_share_data->img_pre_class[i] <<endl;
        //     }

        //     for(int i=0; i<share_men_instance.p_share_data->img_pre_object_size; i ++){
        //         cout<<"pre 2dbbox  "<< share_men_instance.p_share_data->img_pre_2dbbox[i *4]<<" "<<
        //         share_men_instance.p_share_data->img_pre_2dbbox[i*4 + 1]<<" "<<
        //         share_men_instance.p_share_data->img_pre_2dbbox[i*4 + 2 ]<<" "<<
        //         share_men_instance.p_share_data->img_pre_2dbbox[i*4 + 3]<<" "<<endl;
        //     }

        //     cv::Mat cvoutImg = cv::Mat(480,640,CV_8UC1);
        //     memcpy((char*)cvoutImg.data, share_men_instance.p_share_data->img_pre_mask  + 2* 480*640,480*640);
        //     cout<<"num "<<cv::countNonZero(cvoutImg)<<endl;
        //     cvoutImg = cvoutImg*255;
        //     cv::imshow("RecData_Show", cvoutImg);
        //     waitKey(0);
        //     share_men_instance.p_share_data->img_flag = 4;
        // }
        
        // if(share_men_instance.p_share_data->img_flag == 0 && share_men_instance.p_share_data->pc_flag == 0){
        //     t1 = std::chrono::system_clock::now();
        //     // send_pc();
        //     // send_img();
        //     thread th1(send_pc);
        //     thread th2(send_img);
        //     th1.join();
        //     th2.join();
        // }
        // if(share_men_instance.p_share_data->img_flag == 2 && share_men_instance.p_share_data->pc_flag == 2){
        //     t2 = std::chrono::system_clock::now();
        //     cout<<std::chrono::duration_cast<std::chrono::microseconds>( t2-t1 ).count()<<std::endl;
        //     break;
        // }

    }



    return 0;
}
